using Engine;
using Engine.FileFormats3D.WR2TrafficFiles;
using Engine.TrafficHelpers;
using Scenario_Editor.RoadBuilder;
using SharpDX;
using System;
using System.Collections.Generic;
using System.IO;

namespace WR2Streetformat.WR2TrafficFiles
{
    internal class WR2TrafficFile
    {
        private CrossingBlockManager crossingBlockManager;
        private char[] fileIdenty;
        private LaneFileBlock lanes;
        private SpeedBlockFileBlock speedBlocks;
        private SplineDataFileBlock splineData;
        private SplineFileBlock splines;
        private StreetNodeFileBlock streetNodes;
        private int version;

        public WR2TrafficFile(RoadManager roadbuilder)
            : this()
        {
            //Adds the normal Roads to the street
            for (int st = 0; st < roadbuilder.RoadArray.Count; st++)
            {
                Roadpiece road = roadbuilder.RoadArray[st];
                AddRoadpiece(road, roadbuilder);
            }

            //Adds the crossroads to the file
            for (int cr = 0; cr < roadbuilder.streetPoint.Count; cr++)
            {
                AddCrossroads(roadbuilder.streetPoint[cr], cr);
            }

            this.splines.UpdateNumberOfWays(this.streetNodes.Count());

            RemoveDeadEndTracks();
            RemoveUnusedNodes();

            this.splines.UpdateNumberOfWays(this.streetNodes.Count());
            UpdateCrossingBlock();
            //Create Standard Spline Blocks
            //CreateStatndardSplineBlocks();
        }

        public WR2TrafficFile()
        {
            crossingBlockManager = new CrossingBlockManager();
            lanes = new LaneFileBlock();
            streetNodes = new StreetNodeFileBlock();
            splines = new SplineFileBlock();
            speedBlocks = new SpeedBlockFileBlock();
            splineData = new SplineDataFileBlock();

            fileIdenty = new char[] { 'N', 'R', 'T', 'S' };
            version = 258;
        }

        public void Save(string filename)
        {
            FileStream fs = new FileStream(filename, FileMode.Create);
            BinaryWriter bw = new BinaryWriter(fs);

            //Header of the TrafficFile
            bw.Write(fileIdenty);
            bw.Write(version);

            bw.Write(this.lanes.Count());
            bw.Write(this.streetNodes.Count());
            bw.Write(this.splines.Count());
            bw.Write(this.speedBlocks.Count());
            bw.Write(this.splineData.Count());

            //        10 Anzahl der Strassentyen
            this.lanes.Save(bw);
            //        200 Anzahl der Strassen Nodes
            this.streetNodes.Save(bw);
            //        480 Anzahl der Strassen
            this.splines.Save(bw);
            //        452  Anzahl Der Speedblocks
            this.speedBlocks.Save(bw);
            //        390  Anzahl Der Splines
            this.splineData.Save(bw);

            bw.Close();
            fs.Close();
        }

        private void AddCrossroads(Scenario_Editor.RoadBuilder.Streetpoint streetpointInput, int crossID)
        {
            Streetpoint streetpoint = streetpointInput;
            Vector3 originPosition = streetpoint.pos;
            if (streetpoint.positions.Count > 2 && streetpoint.CrossroadTraffic.Length > 2) // Check if this is a crossroad
            {
                CrossingBlock crossBlock = new CrossingBlock(this.crossingBlockManager.blocks.Count - 1);
                SortStreetpointDataByAngle(ref streetpoint, ref originPosition);

                for (int i = 0; i < streetpoint.CrossroadTraffic.Length; i++)
                {
                    int nodeStart = this.streetNodes.GetNode(streetpoint.pos + streetpoint.positions[i]);

                    // Set the mainstreet info
                    StreetNode streetNode = this.streetNodes.Streetnodes[nodeStart];
                    streetNode.IsPriorityStreet = streetpoint.CrossroadTraffic[i].IsMainstreet;

                    this.streetNodes.Streetnodes[nodeStart] = streetNode;

                    int j;
                    TurnType turnType = TurnType.None;
                    if (streetpoint.CrossroadTraffic[i].LaneRight.Enable) // Turn Right
                    {
                        float tangentLengthStart = streetpoint.CrossroadTraffic[i].LaneRight.TangentLengthStart * 1.50f;
                        float tangentLengthEnd = streetpoint.CrossroadTraffic[i].LaneRight.TangentLengthEnd * 1.50f;
                        double angle = streetpoint.CrossroadTraffic[i].StreetNode.GetDirectionInDegree();
                        double angleFromStreetpoint = this.streetNodes.GetDirectionInDegree(nodeStart);

                        if ((angle > 180.0f) == (angleFromStreetpoint > 180.0f) || (angleFromStreetpoint > 180 != (angle > 180.0f))) // Get the right Tangent direction
                        {
                            tangentLengthStart = -tangentLengthStart;
                        }

                        turnType = (streetpoint.positions.Count > 3) ? TurnType.Right : TurnType.Forward;
                        j = GeoMath.GetNextId(i, streetpoint.positions.Count);

                        AddTurnLane(ref streetpoint, streetpoint.CrossroadTraffic[i].LaneRight, crossID, ref originPosition, crossBlock, nodeStart, tangentLengthStart, tangentLengthEnd, j, i, turnType);
                    }

                    if (streetpoint.positions.Count > 3 && streetpoint.CrossroadTraffic[i].LaneForward.Enable) // Forward
                    {
                        float tangentLengthStart = streetpoint.CrossroadTraffic[i].LaneLeft.TangentLengthStart * 1.50f;
                        float tangentLengthEnd = streetpoint.CrossroadTraffic[i].LaneLeft.TangentLengthEnd * 1.50f;
                        double angle = streetpoint.CrossroadTraffic[i].StreetNode.GetDirectionInDegree();
                        double angleFromStreetpoint = this.streetNodes.GetDirectionInDegree(nodeStart);

                        if ((angle > 180.0f) == (angleFromStreetpoint > 180.0f) || (angleFromStreetpoint > 180 != (angle > 180.0f))) // Get the right Tangent direction
                        {
                            tangentLengthStart = -tangentLengthStart;
                        }

                        turnType = (streetpoint.positions.Count > 3) ? TurnType.Right : TurnType.Forward;

                        j = GeoMath.GetNextId(GeoMath.GetNextId(i, streetpoint.positions.Count), streetpoint.positions.Count);

                        AddTurnLane(ref streetpoint, streetpoint.CrossroadTraffic[i].LaneForward, crossID, ref originPosition, crossBlock, nodeStart, tangentLengthStart, tangentLengthEnd, j, i, turnType);
                    }

                    if (streetpoint.CrossroadTraffic[i].LaneLeft.Enable) // Turn Right
                    {
                        float tangentLengthStart = streetpoint.CrossroadTraffic[i].LaneLeft.TangentLengthStart * 1.50f;
                        float tangentLengthEnd = streetpoint.CrossroadTraffic[i].LaneLeft.TangentLengthEnd * 1.50f;
                        double angle = streetpoint.CrossroadTraffic[i].StreetNode.GetDirectionInDegree();
                        double angleFromStreetpoint = this.streetNodes.GetDirectionInDegree(nodeStart);

                        if ((angle > 180.0f) == (angleFromStreetpoint > 180.0f) || (angleFromStreetpoint > 180 != (angle > 180.0f))) // Get the right Tangent direction
                        {
                            tangentLengthStart = -tangentLengthStart;
                        }

                        turnType = (streetpoint.positions.Count > 3) ? TurnType.Left : TurnType.Forward;
                        j = GeoMath.GetPreviousId(i, streetpoint.positions.Count);

                        AddTurnLane(ref streetpoint, streetpoint.CrossroadTraffic[i].LaneLeft, crossID, ref originPosition, crossBlock, nodeStart, tangentLengthStart, tangentLengthEnd, j, i, turnType);
                    }
                }

                this.crossingBlockManager.AddCroosingBlock(crossBlock);
            }
        }

        private bool AddCrossroadStreet(int nodeStart, int nodeEnd, int usedSpeedBlock, float tanhentLengthStart, float tangentLengthEnd, int crossID, TurnType turnType)
        {
            // Check if the street not already exist
            for (int i = 0; i < this.splines.splines.Count; i++)
            {
                if (this.splines.splines[i].nodestart1 == nodeStart &&
                    this.splines.splines[i].nodeEnd2 == nodeEnd)
                {
                    return false;
                }
            }

            float streetLength = GeoMath.DistanceBtw2Points(new Vector3(this.streetNodes.Streetnodes[nodeStart].positionX, this.streetNodes.Streetnodes[nodeStart].positionY, this.streetNodes.Streetnodes[nodeStart].positionZ),
                 new Vector3(this.streetNodes.Streetnodes[nodeEnd].positionX, this.streetNodes.Streetnodes[nodeEnd].positionY, this.streetNodes.Streetnodes[nodeEnd].positionZ));

            for (int i = 0; i < this.splines.splines.Count; i++)
            {
                if (this.splines.splines[i].nodestart1 == nodeEnd &&
                    this.splines.splines[i].crossroad == 0)
                {
                    splines.AddStreet(nodeStart, nodeEnd, usedSpeedBlock, tanhentLengthStart, tangentLengthEnd, streetLength, (short)EnumCrossroad.Crossroad.yes, crossID, turnType);
                    return true;
                }
            }
            return false;
        }

        private void AddRoadpiece(Roadpiece road, RoadManager roadbuilder)
        {
            int nodeStart = -1;
            int nodeEnd = -1;
            Streetpoint streetNode = roadbuilder.streetPoint[road.SPOne];
            Vector3 origin = streetNode.pos;
            float tangentLengthStart = 0.0f;
            float tangentLengthEnd = 0.0f;

            if (roadbuilder.streetPoint[road.SPOne].positions.Count > 2)
            {
                origin = roadbuilder.streetPoint[road.SPOne].GetPosition(road.ID) + roadbuilder.streetPoint[road.SPOne].pos;
            }

            // Find or add Streetnode Start
            nodeStart = this.streetNodes.GetNode(origin);
            tangentLengthStart = GeoMath.DistancePlanar(origin, origin + streetNode.GetTangent(road.ID)) * 3.0f;
            double angle = GeoMath.GetXYAngel(origin, origin + streetNode.GetTangent(road.ID));

            if (nodeStart == -1)// when no Streetpoint is found add a new One
            {
                nodeStart = streetNodes.Count();
                double cos = Math.Cos(angle * (Math.PI / 180));
                double sin = Math.Sin(angle * (Math.PI / 180));
                // Get the inclination angle in radians. When the node is a crossroad it is set to zero.
                float inclinationRadiands = (streetNode.usedbyID.Count > 2) ? 0.0f : -(float)(GeoMath.ConvertDegreeToRadian(streetNode.RoadInclinationAngle));
                streetNodes.AddStreetNode(origin.X, origin.Y, origin.Z, (float)sin, (float)cos, inclinationRadiands);
            }
            else
            {
                //Get the correct Tangent Length (Positive or negative);
                double tesT = (streetNodes.GetDirectionInDegree(nodeStart));
                if ((angle > 180.0f) == (streetNodes.GetDirectionInDegree(nodeStart) > 180.0f) || tesT > 180 != (angle > 180.0f))
                {
                    tangentLengthStart = -tangentLengthStart;
                }
            }

            // Find or add Streetnode End
            streetNode = roadbuilder.streetPoint[road.SPTwo];
            origin = roadbuilder.streetPoint[road.SPTwo].pos;
            if (roadbuilder.streetPoint[road.SPTwo].positions.Count > 2)
            {
                origin = roadbuilder.streetPoint[road.SPTwo].GetPosition(road.ID) + roadbuilder.streetPoint[road.SPTwo].pos;
            }

            nodeEnd = this.streetNodes.GetNode(origin);
            tangentLengthEnd = GeoMath.DistancePlanar(origin, origin + streetNode.GetTangent(road.ID)) * 3.0f;
            angle = GeoMath.GetXYAngel(origin, origin + streetNode.GetTangent(road.ID));

            if (nodeEnd == -1)// when no Streetpoint is found add a new One
            {
                nodeEnd = streetNodes.Count();
                double cos = Math.Cos(angle * (Math.PI / 180));
                double sin = Math.Sin(angle * (Math.PI / 180));
                // Get the inclination angle in radians. When the node is a crossroad it is set to zero.
                float inclinationRadiands = (streetNode.usedbyID.Count > 2) ? 0.0f : -(float)(GeoMath.ConvertDegreeToRadian(streetNode.RoadInclinationAngle));

                streetNodes.AddStreetNode(origin.X, origin.Y, origin.Z, (float)sin, (float)cos, inclinationRadiands);
            }
            else
            {
                //Get the correct Tangent Length (Positive or negative);
                if ((angle > 180.0f) == (streetNodes.GetDirectionInDegree(nodeEnd) > 180.0f) || streetNodes.GetDirectionInDegree(nodeEnd) > 180.0 != (angle > 180.0f))
                {
                    tangentLengthEnd = -tangentLengthEnd;
                }
            }

            if (road.Traffic.hasLaneRight)
            {
                AddStreet(nodeStart, nodeEnd, this.speedBlocks.GetSpeedBlockID(road.Traffic.speedRightLane, this.lanes.GetLaneIdOrAddNewOne(road.Traffic.LaneRight)), -tangentLengthStart, tangentLengthEnd);
            }

            if (road.Traffic.hasLaneLeft)
            {
                AddStreet(nodeEnd, nodeStart, this.speedBlocks.GetSpeedBlockID(road.Traffic.speedLeftLane, this.lanes.GetLaneIdOrAddNewOne(road.Traffic.LaneLeft)), -tangentLengthEnd, tangentLengthStart);
            }
        }

        private void AddStreet(int nodeStart, int nodeEnd, int usedSpeedBlock, float tanhentLengthStart, float tangentLengthEnd)
        {
            // Calculate Strettlength
            float streetLength = GeoMath.DistanceBtw2Points(new Vector3(this.streetNodes.Streetnodes[nodeStart].positionX, this.streetNodes.Streetnodes[nodeStart].positionY, this.streetNodes.Streetnodes[nodeStart].positionZ),
                 new Vector3(this.streetNodes.Streetnodes[nodeEnd].positionX, this.streetNodes.Streetnodes[nodeEnd].positionY, this.streetNodes.Streetnodes[nodeEnd].positionZ));

            splines.AddStreet(nodeStart, nodeEnd, usedSpeedBlock, tanhentLengthStart, tangentLengthEnd, streetLength, 0, -1, TurnType.None);
            StreetNode streetNode = this.streetNodes.Streetnodes[nodeStart];
            streetNode.laneside |= LaneSide.Right;
            this.streetNodes.Streetnodes[nodeStart] = streetNode;

            streetNode = this.streetNodes.Streetnodes[nodeEnd];
            streetNode.laneside |= LaneSide.Left;
            this.streetNodes.Streetnodes[nodeEnd] = streetNode;
        }

        private void AddTurnLane(ref Scenario_Editor.RoadBuilder.Streetpoint streetpoint, CrossroadTrafficData crossroadData, int crossID, ref Vector3 originPosition, CrossingBlock crossBlock, int nodeStart, float tangentLengthStart, float tangentLengthEnd, int j, int i, TurnType turnType)
        {
            int nodeEnd = this.streetNodes.GetNode(streetpoint.pos + streetpoint.positions[j]);
            double angle = GeoMath.GetXYAngel(originPosition, originPosition + streetpoint.Tangent[j]);
            double angleFromStreetpoint = streetNodes.GetDirectionInDegree(nodeEnd);

            if ((angle > 180.0f) == (angleFromStreetpoint > 180.0f) || angleFromStreetpoint > 180 != (angle > 180.0f))
            {
                tangentLengthEnd = -tangentLengthEnd;
            }

            if ((this.streetNodes.Streetnodes[nodeEnd].laneside & LaneSide.Right) == LaneSide.Right && (this.streetNodes.Streetnodes[nodeStart].laneside & LaneSide.Left) == LaneSide.Left ||
                (this.streetNodes.Streetnodes[nodeEnd].laneside & LaneSide.Left) == LaneSide.Left && (this.streetNodes.Streetnodes[nodeStart].laneside & LaneSide.Right) == LaneSide.Right)
            {
                int usedSpeedBlock = this.speedBlocks.GetSpeedBlockID((byte)crossroadData.Speed, this.lanes.GetLaneIdOrAddNewOne(crossroadData.Lane));
                if (AddCrossroadStreet(nodeStart, nodeEnd, usedSpeedBlock, -tangentLengthStart, tangentLengthEnd, crossID, turnType))
                {
                    crossBlock.AddSplineID(this.splines.splines.Count - 1);
                }
            }
        }

        private TurnType CalculateTurnType(Spline spline)
        {
            TurnType turnType = TurnType.Forward;
            double angle1 = GeoMath.GetXYAngel(this.streetNodes.Streetnodes[spline.nodestart1].Position,
              this.streetNodes.Streetnodes[spline.nodestart1].Position + this.streetNodes.Streetnodes[spline.nodestart1].GetPositionFromTangent(spline.tangentLength1));
            double angle2 = GeoMath.GetXYAngel(this.streetNodes.Streetnodes[spline.nodeEnd2].Position,
              this.streetNodes.Streetnodes[spline.nodeEnd2].Position + this.streetNodes.Streetnodes[spline.nodeEnd2].GetPositionFromTangent(spline.tangentLength2));
            double differenz = angle1 - angle2;

            if (differenz < 0)
                differenz += 360;
            if (differenz > 360)
                differenz -= 360;

            if ((angle1 - angle2) < 90.0f && differenz < 120)
                turnType = TurnType.Left;
            if ((angle1 - angle2) < -90.0f && differenz < 120)
                turnType = TurnType.Right;
            return turnType;
        }

        private bool ContainsLeftLane(int nodeStart)
        {
            return this.splines.ContainsLeftLane(nodeStart);
        }

        private bool ContainsRightLane(int nodeEnd)
        {
            return this.splines.ContainsRightLane(nodeEnd);
        }

        private void CreateStatndardSplineBlocks()
        {
            for (int i = 0; i < this.splines.splines.Count; i++)
            {
                this.splineData.AddSplineData(0, (byte)(255), (byte)(255), 0xFF, 0xFF);
            }
        }

        private bool GetIntersection(Vector3[] laneSpline1, Vector3[] laneSpline2)
        {
            int startPoint = 0;

            GeoMath.LineIntersectionResult result = GeoMath.LineIntersectionResult.NoIntersection;
            for (int i = startPoint; i < laneSpline1.Length - 2; i++)
            {
                for (int j = 0; j < laneSpline2.Length - 2; j++)
                {
                    if (Vector3.Zero != GeoMath.GetIntersection(laneSpline1[i], laneSpline1[i + 1], laneSpline2[j], laneSpline2[j + 1], out result))
                    {
                        return true;
                    }
                }
            }
            return false;
        }

        private bool IntersectLanes(Spline spline, Spline spline_2)
        {
            decimal angleSpline1Start = (decimal)GeoMath.GetXYAngel(Vector3.Zero, this.streetNodes.Streetnodes[spline.nodestart1].GetPositionFromTangent(spline.tangentLength1));
            decimal angleSpline1End = (decimal)GeoMath.GetXYAngel(Vector3.Zero, this.streetNodes.Streetnodes[spline.nodeEnd2].GetPositionFromTangent(spline.tangentLength2));
            decimal angleSpline2Start = (decimal)GeoMath.GetXYAngel(Vector3.Zero, this.streetNodes.Streetnodes[spline_2.nodestart1].GetPositionFromTangent(spline_2.tangentLength1)); ;
            decimal angleSpline2End = (decimal)GeoMath.GetXYAngel(Vector3.Zero, this.streetNodes.Streetnodes[spline_2.nodeEnd2].GetPositionFromTangent(spline_2.tangentLength2)); ;

            Vector3 spline1StartPoint = this.streetNodes.Streetnodes[spline.nodestart1].Position + GeoMath.MakePointFromAngel(180 - angleSpline1Start, 5.2f, 0.0f);
            Vector3 spline1EndPoint = this.streetNodes.Streetnodes[spline.nodeEnd2].Position + GeoMath.MakePointFromAngel(180 - angleSpline1End, 5.2f, 0.0f);
            Vector3 spline2StartPoint = this.streetNodes.Streetnodes[spline_2.nodestart1].Position + GeoMath.MakePointFromAngel(180 - angleSpline2Start, 5.2f, 0.0f);
            Vector3 spline2EndPoint = this.streetNodes.Streetnodes[spline_2.nodeEnd2].Position + GeoMath.MakePointFromAngel(180 - angleSpline2End, 5.2f, 0.0f);

            if (spline1StartPoint == spline2StartPoint)
                return false;

            Vector3[] controlPointsSpline1 = new Vector3[4];
            controlPointsSpline1[3] = spline1EndPoint - this.streetNodes.Streetnodes[spline.nodestart1].GetPositionFromTangent(spline.tangentLength1);
            controlPointsSpline1[2] = spline1StartPoint;
            controlPointsSpline1[1] = spline1EndPoint;
            controlPointsSpline1[0] = spline1StartPoint + this.streetNodes.Streetnodes[spline.nodeEnd2].GetPositionFromTangent(spline.tangentLength2);

            Vector3[] controlPointsSpline2 = new Vector3[4];
            controlPointsSpline2[3] = spline2EndPoint - this.streetNodes.Streetnodes[spline_2.nodestart1].GetPositionFromTangent(spline_2.tangentLength1);
            controlPointsSpline2[2] = spline2StartPoint;
            controlPointsSpline2[1] = spline2EndPoint;
            controlPointsSpline2[0] = spline2StartPoint + this.streetNodes.Streetnodes[spline_2.nodeEnd2].GetPositionFromTangent(spline_2.tangentLength2);

            Vector3[] laneSpline1 = GeoMath.CreateMiddleLine(controlPointsSpline1, 1.0f);
            Vector3[] laneSpline2 = GeoMath.CreateMiddleLine(controlPointsSpline2, 1.0f);

            return GetIntersection(laneSpline1, laneSpline2);
        }

        private void RemoveDeadEndTracks()
        {
            bool allDeleated = true;
            do
            {
                allDeleated = true;

                /* Remove Dead end tracks */
                for (int i = 0; i < this.splines.splines.Count; i++)
                {
#if DEBUG
                    List<Spline> discontiniousSplines = new List<Spline>();
                    List<Spline> crossRoadsSplines = new List<Spline>();
                    for (int j = 0; j < this.splines.splines.Count; j++)
                    {
                        if (this.splines.splines[j].nextSpline == -1)
                            discontiniousSplines.Add(this.splines.splines[j]);
                        if (this.splines.splines[j].crossroad == 1)
                            crossRoadsSplines.Add(this.splines.splines[j]);
                    }
#endif
                    if (this.splines.splines[i].nextSpline == -1)
                    {
                        allDeleated = false;

                        int index = i;
                        // find the splines which start at the same spline which will be deleted.
                        List<int> splinesOnCrossroads = new List<int>();
                        for (int j = 0; j < this.splines.splines.Count; j++)
                        {
                            if (index != j
                                && this.splines.splines[index].previewSpline != -1
                                && this.splines.splines[index].previewSpline == this.splines.splines[j].previewSpline)
                            {
                                splinesOnCrossroads.Add(j);
                            }
                        }
                        if (this.splines.splines[index].previewSpline > -1)
                        {
                            if (splinesOnCrossroads.Count > 0)
                                this.splines.splines[this.splines.splines[index].previewSpline].nextSpline = (short)splinesOnCrossroads[0];
                            else
                                this.splines.splines[this.splines.splines[index].previewSpline].nextSpline = -1;
                        }
                        else // the spline could start behind a crossroad. At those splines the previous spline ID is '-1'
                        {
                            for (int j = 0; j < this.splines.splines.Count; j++)
                            {
                                if (this.splines.splines[j].nextSpline == index)
                                {
                                    this.splines.splines[j].nextSpline = -1;
                                }
                            }
                        }

                        // Update the spline Ids for all other splines. becouse the spline will deleted
                        for (int j = 0; j < this.splines.splines.Count; j++)
                        {
                            if (splines.splines[j].oppositeLane == index) // When it is the opposite Lane. The id will be deleted.
                            {
                                splines.splines[j].oppositeLane = -1;
                                i = 0;
                            }
                        }

                        for (int j = 0; j < this.splines.splines.Count; j++)
                        {
                            if (splines.splines[j].oppositeLane > index)
                                splines.splines[j].oppositeLane--;

                            if (splines.splines[j].nextSpline > index)
                                splines.splines[j].nextSpline--;

                            if (splines.splines[j].previewSpline > index)
                                splines.splines[j].previewSpline--;

                            if (splines.splines[j].index > index)
                                splines.splines[j].index--;
                        }

                        // Update the crossroad Ids.
                        for (int k = 0; k < this.crossingBlockManager.blocks.Count; k++)
                        {
                            for (int l = 0; l < this.crossingBlockManager.blocks[k].splineIDs.Count; l++)
                            {
                                if (this.crossingBlockManager.blocks[k].splineIDs[l] > index)
                                    this.crossingBlockManager.blocks[k].splineIDs[l]--;
                            }
                        }
                        for (int k = 0; k < this.crossingBlockManager.blocks.Count; k++)
                        {
                            for (int l = 0; l < this.crossingBlockManager.blocks[k].splineIDs.Count; l++)
                            {
                                if (this.crossingBlockManager.blocks[k].splineIDs[l] == index)
                                {
                                    this.crossingBlockManager.blocks[k].splineIDs.RemoveAt(l);
                                    l--;
                                    continue;
                                }
                            }
                        }
                        this.splines.splines.RemoveAt(index);
                        i--;
                    }
                }
            } while (!allDeleated);

            // Remove unused crossingBlocks
            for (int i = 0; i < this.crossingBlockManager.blocks.Count; i++)
            {
                if (this.crossingBlockManager.blocks[i].splineIDs.Count < 2)
                {
                    for (int j = 0; j < this.crossingBlockManager.blocks[i].splineIDs.Count; j++)
                    {
                        int id = this.crossingBlockManager.blocks[i].splineIDs[j];
                        int nextID = this.splines.splines[id].nextSpline;
                        this.splines.splines[nextID].previewSpline = (short)id;
                    }
                }
                if (this.crossingBlockManager.blocks[i].splineIDs.Count == 0)
                {
                    this.crossingBlockManager.blocks.RemoveAt(i);
                    i--;
                }
            }
        }

        private void RemoveUnusedNodes()
        {
            List<int> listOfUsedStreetnode = new List<int>();

            // Find all used streetnodes
            for (int i = 0; i < this.splines.splines.Count; i++)
            {
                if (!listOfUsedStreetnode.Contains(this.splines.splines[i].nodestart1))
                    listOfUsedStreetnode.Add(this.splines.splines[i].nodestart1);
                if (!listOfUsedStreetnode.Contains(this.splines.splines[i].nodeEnd2))
                    listOfUsedStreetnode.Add(this.splines.splines[i].nodeEnd2);
            }

            // Remove unused streetnodes and update the references in the spline properties.
            for (int i = 0; i < this.streetNodes.Streetnodes.Count; i++)
            {
                int index = i;
                if (!listOfUsedStreetnode.Contains(i))
                {
                    for (int j = 0; j < this.splines.splines.Count; j++)
                    {
                        if (this.splines.splines[j].nodestart1 > index)
                            this.splines.splines[j].nodestart1--;
                        if (this.splines.splines[j].nodeEnd2 > index)
                            this.splines.splines[j].nodeEnd2--;
                    }

                    for (int j = 0; j < listOfUsedStreetnode.Count; j++)
                    {
                        if (listOfUsedStreetnode[j] > index)
                            listOfUsedStreetnode[j]--;
                    }

                    this.streetNodes.Streetnodes.RemoveAt(index);
                    i--;
                }
            }
        }

        private static void SortStreetpointDataByAngle(ref Scenario_Editor.RoadBuilder.Streetpoint streetpoint, ref Vector3 originPosition)
        {
            List<Vector3> positions = new List<Vector3>(streetpoint.positions);
            List<int> usedbyID = new List<int>(streetpoint.usedbyID);
            List<decimal> angles = new List<decimal>(streetpoint.angle);
            List<Vector3> tangents = new List<Vector3>(streetpoint.Tangent);
            List<CrossroadTraffic> crossroadTraffic = new List<CrossroadTraffic>(streetpoint.CrossroadTraffic);

            //for (int i = 0; i < angles.Count; i++)
            //{
            //    if (angles[i] >= 360.0m)
            //    {
            //        angles[i] -= 360.0m;
            //        i--;
            //        continue;
            //    }
            //    if (angles[i] < 0)
            //    {
            //        angles[i] += 360.0m;
            //        i--;
            //        continue;
            //    }
            //}

            bool sortiert = true;
            // Sort streetpoints by angles.
            do
            {
                sortiert = true;
                for (int i = 0; i < streetpoint.positions.Count - 1; i++)
                {
                    double angle = GeoMath.GetXYAngel(originPosition, originPosition + positions[i]);
                    double angle2 = GeoMath.GetXYAngel(originPosition, originPosition + positions[i + 1]);
                    if (angle < angle2)
                    {
                        Vector3 position = positions[i];
                        CrossroadTraffic crossroadTrafficTemp = crossroadTraffic[i];

                        Vector3 tangent = tangents[i];
                        decimal angleTemp = angles[i];
                        int usedById = usedbyID[i];

                        positions[i] = positions[i + 1];
                        crossroadTraffic[i] = crossroadTraffic[i + 1];
                        tangents[i] = tangents[i + 1];
                        angles[i] = angles[i + 1];
                        usedbyID[i] = usedbyID[i + 1];

                        positions[i + 1] = position;
                        crossroadTraffic[i + 1] = crossroadTrafficTemp;
                        tangents[i + 1] = tangent;
                        angles[i + 1] = angleTemp;
                        usedbyID[i + 1] = usedById;
                        sortiert = false;
                    }
                }
            } while (!sortiert);

            streetpoint.CrossroadTraffic = crossroadTraffic.ToArray();
            streetpoint.angle = angles;
            streetpoint.Tangent = tangents;
            streetpoint.positions = positions;
            streetpoint.usedbyID = usedbyID;
        }

        private void UpdateCrossingBlock()
        {
            List<IntersectingSplines> intersectionSplines = new List<IntersectingSplines>();
            for (int i = 0; i < this.crossingBlockManager.blocks.Count; i++)
            {
                CrossingBlock blockCR = this.crossingBlockManager.blocks[i];
                for (int j = 0; j < blockCR.splineIDs.Count; j++)
                {
                    if (20 == blockCR.splineIDs[j])
                    { }
                    Spline spline = this.splines.splines[blockCR.splineIDs[j]];

                    IntersectingSplines intersectionForSpline = new IntersectingSplines(blockCR.splineIDs[j]);
                    intersectionSplines.Add(intersectionForSpline);

                    for (int k = 0; k < blockCR.splineIDs.Count; k++)
                    {
                        if (j != k)
                        {
                            //Check if they intersect eachother
                            if (IntersectLanes(this.splines.splines[blockCR.splineIDs[j]], this.splines.splines[blockCR.splineIDs[k]]))
                            {
                                intersectionForSpline.AddIntersectingSplineID(blockCR.splineIDs[k]); //add the next splineID when they intersect eachother
                            }
                        }
                    }
                }
            }

            for (int i = 0; i < intersectionSplines.Count; i++)
            {
                Spline splineTemp = this.splines.splines[intersectionSplines[i].SplineID];
                // check if the street is not a mainstreet
                if (!(this.streetNodes.Streetnodes[splineTemp.nodeEnd2].IsPriorityStreet &&
                    this.streetNodes.Streetnodes[splineTemp.nodestart1].IsPriorityStreet))
                {
                    IntersectingSplines block = intersectionSplines[i];
                    if (block.IntersectingSplineIds.Count > 0)
                    {
                        int offset = block.IntersectingSplineIds.Count - 1;
                        int splineID, numberOfTrunLeft = 0;
                        bool containsforwardSrteet = false;
                        for (int j = 0; j < block.IntersectingSplineIds.Count; j++)
                        {
                            splineID = block.IntersectingSplineIds[j];
                            Spline spline = this.splines.splines[splineID];

                            if (spline.turnType == TurnType.Forward)
                            {
                                containsforwardSrteet = true;
                                break;
                            }
                            if (spline.turnType == TurnType.Left)
                                numberOfTrunLeft++;
                        }

                        Spline masterSpline = this.splines.splines[block.SplineID];
                        for (int j = 0; j < block.IntersectingSplineIds.Count; j++)
                        {
                            splineID = block.IntersectingSplineIds[j];
                            short test = (short)(255 / block.IntersectingSplineIds.Count);
                            Spline spline = this.splines.splines[splineID];

                            if (!this.streetNodes.Streetnodes[masterSpline.nodestart1].IsPriorityStreet &&
                                this.streetNodes.Streetnodes[spline.nodestart1].IsPriorityStreet &&
                                numberOfTrunLeft >= 2)
                            {
                                this.splineData.AddSplineData(splineID, (byte)255, (byte)255, 0xFF, 0xFF);
                                numberOfTrunLeft--;
                            }
                            else
                            {
                                this.splineData.AddSplineData(splineID, (byte)0, (byte)0, 0x00, 0x00);
                            }

                            if (this.splines.splines[block.SplineID].firstRow == -1)
                            {
                                this.splines.splines[block.SplineID].firstRow = (short)(this.splineData.Count() - 1);
                            }
                            this.splines.splines[block.SplineID].numberOfRows++;
                        }
                    }
                }
                else
                {
                }
            }
        }

        private class IntersectingSplines
        {
            private List<int> intersectingSplineIds;
            private int splineID;

            public IntersectingSplines(int splineId)
            {
                this.splineID = splineId;
                this.intersectingSplineIds = new List<int>();
            }

            public List<int> IntersectingSplineIds
            {
                get { return intersectingSplineIds; }
                set { intersectingSplineIds = value; }
            }

            public int SplineID
            {
                get { return splineID; }
                set { splineID = value; }
            }

            public void AddIntersectingSplineID(int splineID)
            {
                if (!this.intersectingSplineIds.Contains(splineID))
                    this.intersectingSplineIds.Add(splineID);
                this.intersectingSplineIds.Sort();
            }
        }
    }
}